/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2017 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_KINEMATICSENSORMEASUREMENT_H_
#define __MMM_KINEMATICSENSORMEASUREMENT_H_

#include "../../SensorMeasurement.h"
#include "../../InterpolatableSensorMeasurement.h"

namespace MMM
{
class KinematicSensorMeasurement;

typedef boost::shared_ptr<KinematicSensorMeasurement> KinematicSensorMeasurementPtr;

class KinematicSensorMeasurement : public InterpolatableSensorMeasurement<KinematicSensorMeasurement>, SMCloneable<KinematicSensorMeasurement>
{

public:
    KinematicSensorMeasurement(float timestep, const Eigen::VectorXf &jointAngles, SensorMeasurementType type = SensorMeasurementType::MEASURED);

    SensorMeasurementPtr clone();

    bool equals(SensorMeasurementPtr sensorMeasurement);

    KinematicSensorMeasurementPtr clone(float newTimestep);

    Eigen::VectorXf getJointAngles();

protected:
    KinematicSensorMeasurementPtr interpolate(KinematicSensorMeasurementPtr other, float timestep, SensorMeasurementType type);

    void appendMeasurementDataXML(RapidXMLWriterNodePtr measurementNode);

private:
    Eigen::VectorXf jointAngles;

};
}
#endif // __MMM_KINEMATICSENSORMEASUREMENT_H_
